

import requests
import logging
import math
import serial
import threading
import time
BAIDU_AK=...
logging.basicConfig(
        level=logging.INFO,
        format="%(asctime)s [%(levelname)s] %(message)s",
        handlers=[logging.FileHandler("app.log"), logging.StreamHandler()]
    )
a=[1]
class navigation():

    def __init__(self,city:str,targetname:str):
        self.ak=BAIDU_AK
        self.city=city
        self.targetname=targetname
        self.currentpos=[]
        self.previouspos=None

        self.gps_get = GET_GPS(port='/dev/ttyUSB0', baudrate=38400, timeout=3)


        
    """
    获取目标位置的gps坐标
    """
    def geocode_address(self, timeout=5):
        base_url = "https://api.map.baidu.com/place/v2/suggestion"
        params = {
            "query": self.targetname,
            "region": self.city,
            "city_limit": "true",
            "output": "json",
            "ak": self.ak,
        }
        try:
            response = requests.get(base_url, params=params, timeout=timeout)
            data = response.json()
            
            if data["status"] == 0:
                if len(data["result"]) > 0:
                    first_result = data["result"][0]  # 取第一个结果
                    if "location" in first_result:    # 检查是否有经纬度
                        self.machine_target = first_result["name"]
                        self.lat = first_result["location"]["lat"]
                        self.lng = first_result["location"]["lng"]
                        return True
                    else:
                        logging.error(f"[geocode] 结果无经纬度数据: {first_result['name']}")
                        return None
                else:
                    logging.error("[geocode] 未找到地理编码结果")
                    return None
            else:
                logging.error(f"[geocode] 地理编码失败: {data['message']} (状态码: {data['status']})")
                return None
                
        except Exception as e:
            logging.error(f"[geocode] API请求异常: {str(e)}")
            return None
        

        
    """
    创建GET_GPS的对象,获取当前的gps坐标
    传给self.currentpos属性
    该方法由navi()方法主动调用
    """
 
    def get_currentpos(self, mode="h"):
        if mode == "debug":
            if not hasattr(self, 'simulated_pos'):
                self.simulated_pos = [30.516562, 114.413131]  # 初始模拟位置
                self.use_simulated_data = False  # 是否使用模拟数据的标志
            start_time = time.time()
            while True:
                # 先尝试获取真实GPS数据
                if self.gps_get.lat is not None and self.gps_get.lng is not None:
                    self.currentpos = [self.gps_get.lat, self.gps_get.lng]
                    self.use_simulated_data = False
                    logging.info(f"获取到GPS数据: {self.currentpos}")
                    break
                if self.use_simulated_data:
                    # self.currentpos[0] += 0.00001
                    # self.currentpos[1] += 0.00001
                    logging.info(f"更新位置: {self.currentpos}")
                    break
                # 2秒后仍未获取到真实数据，开始使用模拟数据
                if time.time() - start_time > 2 and not self.use_simulated_data:
                    self.use_simulated_data = True
                    self.currentpos = self.simulated_pos.copy()
          
                    break
                # 如果正在使用模拟数据，则每次递增
                time.sleep(2)  # 避免CPU占用过高
        else:
            max_attempts = 10  # 最大尝试次数
            attempt = 0
            
            while attempt < max_attempts:
                # 检查GPS数据是否有效
                print(self.gps_get.s)
                if self.gps_get.lat is not None and self.gps_get.lng is not None:
                    self.currentpos = [self.gps_get.lat, self.gps_get.lng]
                
                    logging.info(f"[GPS] 获得有效GPS数据: {self.currentpos}")
                    return  # 成功获取数据，退出循环
                
                logging.info("[GPS] 等待获得正常GPS数据...")
                time.sleep(2)  # 等待2秒再尝试
                attempt += 1
            
            # 如果达到最大尝试次数仍未获取数据
            logging.error("[GPS] 无法获取有效GPS数据，请检查GPS设备")
            self.currentpos = [None, None]  # 设置为无效值
                


    """
    调用get_currentpos获取取当前的gps坐标(开启获取线程，自动更新)
    通过访问self.currentpos得到当前坐标数据
    结合目标点的坐标数据self.targetpos
    生成轨迹信息存储到self.navigation_steps = []和self.routes_length=[]
    用户每次任务只需要调用一次
    """
    def navi(self):
        # if __name__ == "__main__":
        #     self.currentpos=[30.516562,114.413131]#测试用
        # else :
        self.get_currentpos()

        self.previouspos=self.currentpos.copy()

        while self.geocode_address() is None:
            logging.error("[GPS] 解码失败")
            pass
        print()
        self.targetpos=[self.lat,self.lng]
        self.params_navi ={
            "ak": self.ak,
            "origin": f"{self.currentpos[0]},{self.currentpos[1]}",
            "destination": f"{self.targetpos[0]},{self.targetpos[1]}",
            "ret_coordtype":"bd09ll"
            }

        url="https://api.map.baidu.com/direction/v2/walking"
        response = requests.get(url, params=self.params_navi)
        data =response.json()
        if data["status"] == 0:
            steps = data["result"]["routes"][0]["steps"]
            self.navigation_steps = []  # 创建空列表存储导航步骤
            self.routes_length=[]
            logging.info("[GPS] 获得路径如下：")
            for step in steps:
                instruction = step["instructions"].replace("<b>", "").replace("</b>", "")
                step_info = f"前行{step['distance']}米，{instruction}"
                self.navigation_steps.append(step_info)  # 将步骤信息添加到列表
                self.routes_length.append(step['distance'])
            logging.info(self.navigation_steps) 
        else:
            logging.error(f"[GPS] 获取路径失败: {data['message']}") 




    """
    传入阶段性路径的起点经纬度，
    获取当前的经纬度，计算路径距离
    """
    @property
    def Calculate_GPS_Distance(self):
        if None in self.previouspos or None in self.currentpos:
            logging.info("尚未获得有效gps数据,无法进行距离计算")
            return -1
        else:
            previous_lat = self.previouspos[0]
            previous_lng = self.previouspos[1]    
            # 获取当前的经纬度
            current_lat = self.currentpos[0]
            current_lng = self.currentpos[1]
            # 转换为弧度
            previous_lat_rad = math.radians(previous_lat)
            previous_lng_rad = math.radians(previous_lng)
            current_lat_rad = math.radians(current_lat)
            current_lng_rad = math.radians(current_lng)
            # 计算差值
            delta_lat = current_lat_rad - previous_lat_rad
            delta_lng = current_lng_rad - previous_lng_rad
            # Haversine公式
            a = math.sin(delta_lat / 2)**2 + math.cos(previous_lat_rad) * math.cos(current_lat_rad) * math.sin(delta_lng / 2)**2
            c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
            # 地球半径（单位：米）
            R = 6371000
            # 计算距离（单位：米）
            distance = R * c
            return distance


"""
声明对象自动开启线程获取gps坐标
在get_currentpos中被调用
"""
class GET_GPS():
    def __init__(self, port="/dev/ttyUSB0", baudrate=38400, timeout=3):
        self.lat=None
        self.lng=None
        self.port=port
        self.s=0
        try:
            self.ser = serial.Serial(port=port, baudrate=baudrate, timeout=timeout)
            self.thread_getgps = threading.Thread(target=self.listen, daemon=True)
            logging.info(f"已成功连接到 {port}，波特率 {baudrate}")
            self.thread_getgps.start()
            logging.info("[GET_GPS] GPS通信线程开启!")
        except Exception as e:
            logging.error(f"[GET_GPS] 初始GPS串口及线程失败: {e}")

    def stop(self):
        self.thread_getgps.join(timeout=1)
        if self.ser and self.ser.is_open:
            try:
                self.ser.close()
                print(f"串口 {self.port} 已关闭")
            except Exception as e:
                print(f"关闭串口时出错: {e}")

    def listen(self):
        try:
            while True:
                
                line = self.ser.readline().decode('ascii', errors='replace').strip()
                if not line:
                    continue
                s = line.split(',')
                #print(s)
                tag = s[0]
                # 优先尝试解析 $GPGGA
                if tag == "$GPGGA":
                    #print(s)
                    if len(s) < 10:
                        continue  # 字段不足
                    try:
                        # 解析纬度（格式：DDMM.MMMM）
                        lat_str = s[2]  # 纬度字段
                        lat_dir = s[3]   # N/S
                        if not lat_str or lat_dir not in ('N', 'S'):
                            continue

                        lat_deg = float(lat_str[:2])       # 度
                        lat_min = float(lat_str[2:])       # 分
                        lat = lat_deg + lat_min / 60
                        if lat_dir == 'S':
                            lat = -lat

                        # 解析经度（格式：DDDMM.MMMM）
                        lng_str = s[4]  # 经度字段
                        lng_dir = s[5]   # E/W
                        if not lng_str or lng_dir not in ('E', 'W'):
                            continue

                        lng_deg = float(lng_str[:3])      # 度
                        lng_min = float(lng_str[3:])      # 分
                        lng = lng_deg + lng_min / 60
                        if lng_dir == 'W':
                            lng = -lng

                        # 更新坐标
                        self.lat = lat
                        self.lng = lng
                        #logging.info(f"更新坐标: 纬度={lat:.6f}, 经度={lng:.6f}")

                    except (ValueError, IndexError) as e:
                        logging.warning(f"解析 $GPGGA 失败: {e}, 原始数据: {line}")

                # 备用解析 $GPRMC
                if tag == "$GPRMC":
                    if len(s) < 7 or s[2] != 'A':  # 状态需为 'A'（有效）
                        continue

                    try:
                        #print(s)
                        # 解析纬度（格式：DDMM.MMMM）
                        lat_str = s[3]
                        lat_dir = s[4]
                        if not lat_str or lat_dir not in ('N', 'S'):
                            continue

                        lat_deg = float(lat_str[:2])
                        lat_min = float(lat_str[2:])
                        lat = lat_deg + lat_min / 60
                        if lat_dir == 'S':
                            lat = -lat

                        # 解析经度（格式：DDDMM.MMMM）
                        lng_str = s[5]
                        lng_dir = s[6]
                        if not lng_str or lng_dir not in ('E', 'W'):
                            continue

                        lng_deg = float(lng_str[:3])
                        lng_min = float(lng_str[3:])
                        lng = lng_deg + lng_min / 60
                        if lng_dir == 'W':
                            lng = -lng

                        # 更新坐标
                        self.lat = lat
                        self.lng = lng
                        #logging.info(f"更新坐标: 纬度={lat:.6f}, 经度={lng:.6f}")

                    except (ValueError, IndexError) as e:
                        logging.warning(f"解析 $GPRMC 失败: {e}, 原始数据: {line}")

                time.sleep(0.1)  # 适当降低 CPU 占用

        except Exception as e:
            logging.error(f"GPS 监听线程异常: {e}")
        finally:
            if hasattr(self, 'ser') and self.ser.is_open:
                self.ser.close()


if __name__=="__main__":

    # mygps=navigation(
    #     city="湖北武汉",
    #     targetname='华中科技大学图书馆',
    # )
    # #logging.info(f"前往{mygps.machine_target}")
    # mygps.navi()

    s=GET_GPS()
    while True:
        pass
        time.sleep(5)

    
    


import requests
import logging
import math
import serial
import threading
import time
BAIDU_AK="EuJNtsc9xNi8y6ZqZQ7ihJ0s8b1KG1BK"
logging.basicConfig(
        level=logging.INFO,
        format="%(asctime)s [%(levelname)s] %(message)s",
        handlers=[logging.FileHandler("app.log"), logging.StreamHandler()]
    )
a=[1]
class navigation():

    def __init__(self,city:str,targetname:str):
        self.ak=BAIDU_AK
        self.city=city
        self.targetname=targetname
        self.currentpos=[]
        self.previouspos=None

        self.gps_get = GET_GPS(port='/dev/ttyUSB0', baudrate=38400, timeout=3)


        
    """
    获取目标位置的gps坐标
    """
    def geocode_address(self, timeout=5):
        base_url = "https://api.map.baidu.com/place/v2/suggestion"
        params = {
            "query": self.targetname,
            "region": self.city,
            "city_limit": "true",
            "output": "json",
            "ak": self.ak,
        }
        try:
            response = requests.get(base_url, params=params, timeout=timeout)
            data = response.json()
            
            if data["status"] == 0:
                if len(data["result"]) > 0:
                    first_result = data["result"][0]  # 取第一个结果
                    if "location" in first_result:    # 检查是否有经纬度
                        self.machine_target = first_result["name"]
                        self.lat = first_result["location"]["lat"]
                        self.lng = first_result["location"]["lng"]
                        return True
                    else:
                        logging.error(f"[geocode] 结果无经纬度数据: {first_result['name']}")
                        return None
                else:
                    logging.error("[geocode] 未找到地理编码结果")
                    return None
            else:
                logging.error(f"[geocode] 地理编码失败: {data['message']} (状态码: {data['status']})")
                return None
                
        except Exception as e:
            logging.error(f"[geocode] API请求异常: {str(e)}")
            return None
        

        
    """
    创建GET_GPS的对象,获取当前的gps坐标
    传给self.currentpos属性
    该方法由navi()方法主动调用
    """
 
    def get_currentpos(self, mode="debug"):
        if mode == "debug":
            if not hasattr(self, 'simulated_pos'):
                self.simulated_pos = [30.516562, 114.413131]  # 初始模拟位置
                self.use_simulated_data = False  # 是否使用模拟数据的标志
            start_time = time.time()
            while True:
                # 先尝试获取真实GPS数据
                if self.gps_get.lat is not None and self.gps_get.lng is not None:
                    self.currentpos = [self.gps_get.lat, self.gps_get.lng]
                    self.use_simulated_data = False
                    logging.info(f"获取到GPS数据: {self.currentpos}")
                    break
                if self.use_simulated_data:
                    #self.currentpos[0] += 0.00009
                    #self.currentpos[1] += 0.00009
                    logging.info(f"更新模拟位置: {self.currentpos}")
                    break
                # 2秒后仍未获取到真实数据，开始使用模拟数据
                if time.time() - start_time > 2 and not self.use_simulated_data:
                    self.use_simulated_data = True
                    self.currentpos = self.simulated_pos.copy()
                    logging.info(f"获得GPS数据: {self.currentpos}")
                    break
                # 如果正在使用模拟数据，则每次递增
                time.sleep(2)  # 避免CPU占用过高
        else:
            max_attempts = 10  # 最大尝试次数
            attempt = 0
            
            while attempt < max_attempts:
                # 检查GPS数据是否有效
                print(self.gps_get.s)
                if self.gps_get.lat is not None and self.gps_get.lng is not None:
                    self.currentpos = [self.gps_get.lat, self.gps_get.lng]
                
                    logging.info(f"[GPS] 获得有效GPS数据: {self.currentpos}")
                    return  # 成功获取数据，退出循环
                
                logging.info("[GPS] 等待获得正常GPS数据...")
                time.sleep(2)  # 等待2秒再尝试
                attempt += 1
            
            # 如果达到最大尝试次数仍未获取数据
            logging.error("[GPS] 无法获取有效GPS数据，请检查GPS设备")
            self.currentpos = [None, None]  # 设置为无效值
                


    """
    调用get_currentpos获取取当前的gps坐标(开启获取线程，自动更新)
    通过访问self.currentpos得到当前坐标数据
    结合目标点的坐标数据self.targetpos
    生成轨迹信息存储到self.navigation_steps = []和self.routes_length=[]
    用户每次任务只需要调用一次
    """
    def navi(self):
        # if __name__ == "__main__":
        #     self.currentpos=[30.516562,114.413131]#测试用
        # else :
        self.get_currentpos()

        self.previouspos=self.currentpos.copy()

        while self.geocode_address() is None:
            logging.error("[GPS] 解码失败")
            pass
        print()
        self.targetpos=[self.lat,self.lng]
        self.params_navi ={
            "ak": self.ak,
            "origin": f"{self.currentpos[0]},{self.currentpos[1]}",
            "destination": f"{self.targetpos[0]},{self.targetpos[1]}",
            "ret_coordtype":"bd09ll"
            }

        url="https://api.map.baidu.com/direction/v2/walking"
        response = requests.get(url, params=self.params_navi)
        data =response.json()
        if data["status"] == 0:
            steps = data["result"]["routes"][0]["steps"]
            self.navigation_steps = []  # 创建空列表存储导航步骤
            self.routes_length=[]
            logging.info("[GPS] 获得路径如下：")
            for step in steps:
                instruction = step["instructions"].replace("<b>", "").replace("</b>", "")
                step_info = f"前行{step['distance']}米，{instruction}"
                self.navigation_steps.append(step_info)  # 将步骤信息添加到列表
                self.routes_length.append(step['distance'])
            logging.info(self.navigation_steps) 
        else:
            logging.error(f"[GPS] 获取路径失败: {data['message']}") 




    """
    传入阶段性路径的起点经纬度，
    获取当前的经纬度，计算路径距离
    """
    @property
    def Calculate_GPS_Distance(self):
        if None in self.previouspos or None in self.currentpos:
            logging.info("尚未获得有效gps数据,无法进行距离计算")
            return -1
        else:
            previous_lat = self.previouspos[0]
            previous_lng = self.previouspos[1]    
            # 获取当前的经纬度
            current_lat = self.currentpos[0]
            current_lng = self.currentpos[1]
            # 转换为弧度
            previous_lat_rad = math.radians(previous_lat)
            previous_lng_rad = math.radians(previous_lng)
            current_lat_rad = math.radians(current_lat)
            current_lng_rad = math.radians(current_lng)
            # 计算差值
            delta_lat = current_lat_rad - previous_lat_rad
            delta_lng = current_lng_rad - previous_lng_rad
            # Haversine公式
            a = math.sin(delta_lat / 2)**2 + math.cos(previous_lat_rad) * math.cos(current_lat_rad) * math.sin(delta_lng / 2)**2
            c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
            # 地球半径（单位：米）
            R = 6371000
            # 计算距离（单位：米）
            distance = R * c
            return distance


"""
声明对象自动开启线程获取gps坐标
在get_currentpos中被调用
"""
class GET_GPS():
    def __init__(self, port="/dev/ttyUSB0", baudrate=38400, timeout=3):
        self.lat=None
        self.lng=None
        self.port=port
        self.s=0
        try:
            self.ser = serial.Serial(port=port, baudrate=baudrate, timeout=timeout)
            self.thread_getgps = threading.Thread(target=self.listen, daemon=True)
            logging.info(f"已成功连接到 {port}，波特率 {baudrate}")
            self.thread_getgps.start()
            logging.info("[GET_GPS] GPS通信线程开启!")
        except Exception as e:
            logging.error(f"[GET_GPS] 初始GPS串口及线程失败: {e}")

    def stop(self):
        self.thread_getgps.join(timeout=1)
        if self.ser and self.ser.is_open:
            try:
                self.ser.close()
                print(f"串口 {self.port} 已关闭")
            except Exception as e:
                print(f"关闭串口时出错: {e}")

    def listen(self):
        try:
            while True:
                
                line = self.ser.readline().decode('ascii', errors='replace').strip()
                if not line:
                    continue
                s = line.split(',')
                #print(s)
                tag = s[0]
                # 优先尝试解析 $GPGGA
                if tag == "$GPGGA":
                    #print(s)
                    if len(s) < 10:
                        continue  # 字段不足
                    try:
                        # 解析纬度（格式：DDMM.MMMM）
                        lat_str = s[2]  # 纬度字段
                        lat_dir = s[3]   # N/S
                        if not lat_str or lat_dir not in ('N', 'S'):
                            continue

                        lat_deg = float(lat_str[:2])       # 度
                        lat_min = float(lat_str[2:])       # 分
                        lat = lat_deg + lat_min / 60
                        if lat_dir == 'S':
                            lat = -lat

                        # 解析经度（格式：DDDMM.MMMM）
                        lng_str = s[4]  # 经度字段
                        lng_dir = s[5]   # E/W
                        if not lng_str or lng_dir not in ('E', 'W'):
                            continue

                        lng_deg = float(lng_str[:3])      # 度
                        lng_min = float(lng_str[3:])      # 分
                        lng = lng_deg + lng_min / 60
                        if lng_dir == 'W':
                            lng = -lng

                        # 更新坐标
                        self.lat = lat
                        self.lng = lng
                        #logging.info(f"更新坐标: 纬度={lat:.6f}, 经度={lng:.6f}")

                    except (ValueError, IndexError) as e:
                        logging.warning(f"解析 $GPGGA 失败: {e}, 原始数据: {line}")

                # 备用解析 $GPRMC
                if tag == "$GPRMC":
                    if len(s) < 7 or s[2] != 'A':  # 状态需为 'A'（有效）
                        continue

                    try:
                        #print(s)
                        # 解析纬度（格式：DDMM.MMMM）
                        lat_str = s[3]
                        lat_dir = s[4]
                        if not lat_str or lat_dir not in ('N', 'S'):
                            continue

                        lat_deg = float(lat_str[:2])
                        lat_min = float(lat_str[2:])
                        lat = lat_deg + lat_min / 60
                        if lat_dir == 'S':
                            lat = -lat

                        # 解析经度（格式：DDDMM.MMMM）
                        lng_str = s[5]
                        lng_dir = s[6]
                        if not lng_str or lng_dir not in ('E', 'W'):
                            continue

                        lng_deg = float(lng_str[:3])
                        lng_min = float(lng_str[3:])
                        lng = lng_deg + lng_min / 60
                        if lng_dir == 'W':
                            lng = -lng

                        # 更新坐标
                        self.lat = lat
                        self.lng = lng
                        #logging.info(f"更新坐标: 纬度={lat:.6f}, 经度={lng:.6f}")

                    except (ValueError, IndexError) as e:
                        logging.warning(f"解析 $GPRMC 失败: {e}, 原始数据: {line}")

                time.sleep(0.1)  # 适当降低 CPU 占用

        except Exception as e:
            logging.error(f"GPS 监听线程异常: {e}")
        finally:
            if hasattr(self, 'ser') and self.ser.is_open:
                self.ser.close()


if __name__=="__main__":

    # mygps=navigation(
    #     city="湖北武汉",
    #     targetname='华中科技大学图书馆',
    # )
    # #logging.info(f"前往{mygps.machine_target}")
    # mygps.navi()

    s=GET_GPS()
    while True:
        pass
        time.sleep(5)

    